//
// Created by 16933 on 2022/5/1.
//

#include "bsp_MPU6050.h"
#include "bsp_I2C.h"

/*! @brief 写一个字节数据
 * @param reg：写入的寄存器地址
 * @param data：写入的数据
 * @return 1：写入失败 0：写入成功
 * */
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data)
{
    I2C_Start();
    I2C_Send_Byte(MPU_WRITE);
    if (I2C_Wait_ACK())
    {
        I2C_Stop();
        return 1;
    }
    I2C_Send_Byte(reg);
    I2C_Wait_ACK();
    I2C_Send_Byte(data);
    if (I2C_Wait_ACK())
    {
        I2C_Stop();
        return 1;
    }
    I2C_Stop();
    return 0;
}


/*!@brief  读一个字节 数据
 * @param reg：要读的寄存器地址
 * @return 读出的数据
 * */
uint8_t MPU_Read_Byte(uint8_t reg)
{
    uint8_t res;
    I2C_Start();
    I2C_Send_Byte((MPU6050_ADDRESS<<1)|0);       //发送器件地址+写命令
    I2C_Wait_ACK();		                              //等待应答
    I2C_Send_Byte(reg);	                      //写寄存器地址
    I2C_Wait_ACK();		                              //等待应答
    I2C_Start();
    I2C_Send_Byte((MPU6050_ADDRESS<<1)|1);      //发送器件地址+读命令
    I2C_Wait_ACK();		                             //等待应答
    res=I2C_Receive_Byte(0);                          //读取数据,发送nACK
    I2C_Stop();			                             //产生一个停止条件
    return res;
}


/*! @brief 连续写多个字节
 * @param addr：外设地址
 * @param reg：寄存器地址
 * @param len：写入长度
 * @param buf：写入的数据
 * @return 1:失败  0：成功
 * */
uint8_t MPU_Write_Bytes(uint8_t reg,uint8_t len,uint8_t *buf)
{
    uint8_t i;
    I2C_Start();
    I2C_Send_Byte(MPU_WRITE);
    if (I2C_Wait_ACK())
    {
        I2C_Stop();
        return 1 ;
    }
    I2C_Send_Byte(reg);
    I2C_Wait_ACK();
    for (i = 0;i<len;i++) {
        I2C_Send_Byte(buf[i]);
        if (I2C_Wait_ACK())
        {
            I2C_Stop();
            return 1 ;
        }
    }
    I2C_Stop();
    return 0;

}

/*!
 * @brief 连续读取多个字节数据
 * @param addr：外设地址
 * @param reg：寄存器地址
 * @param len：写入长度
 * @param buf：数据存储地址
 * @return 1:失败  0：成功
 * */
uint8_t MPU_Read_Bytes(uint8_t reg,uint8_t len,uint8_t *buf)
{
    I2C_Start();
    I2C_Send_Byte(MPU_WRITE);
    if (I2C_Wait_ACK())
    {
        I2C_Stop();
        return 1 ;
    }
    I2C_Send_Byte(reg);
    I2C_Wait_ACK();
    I2C_Start();
    I2C_Send_Byte(MPU_READ);
    I2C_Wait_ACK();
    while(len)
    {
        if (len==1)
        {
            *buf = I2C_Receive_Byte(0);

        }
        else
        {
            *buf = I2C_Receive_Byte(1);
        }
        len--;
        buf++;

    }
    I2C_Stop();
    return 0;
}

/*!
 * @brief: MPU初始化
 * */
uint8_t MPU_Init()
{
    uint8_t res;
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050
    MPU_Set_Gyro_Fsr(3);					//陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);					//加速度传感器,±2g
    MPU_Set_Rate(50);						//设置采样率50Hz
    MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引脚低电平有效
    res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
    if(res==MPU_ADDR)//器件ID正确
    {
        MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
        MPU_Set_Rate(50);						//设置采样率为50Hz
    }else return 1;
    return 0;
}



/*!
 * @brief 设置MPU6050的数字低通滤波器
 * @param lpf: 数字低通滤波频率(Hz)
 * @return 0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data=0;
    if(lpf>=188)data=1;
    else if(lpf>=98)data=2;
    else if(lpf>=42)data=3;
    else if(lpf>=20)data=4;
    else if(lpf>=10)data=5;
    else data=6;
    return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}


//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败

/*!
 * @brief 设置MPU6050陀螺仪传感器满量程范围
 * @param fsr:  0,±250dps;  1,±500dps;  2,±1000dps; 3,±2000dps
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}



/*!
 * @brief 设置MPU6050加速度传感器满量程范围
 * @param fsr:  0,±2g;1,±4g;2,±8g;3,±16g
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}



/*!
 * @brief 设置MPU6050的采样率(假定Fs=1KHz)
 * @param rate:4~1000(Hz)
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Set_Rate(uint16_t rate)
{
    uint8_t data;
    if(rate>1000)rate=1000;
    if(rate<4)rate=4;
    data=1000/rate-1;
    data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
    return MPU_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}


/*!
 * @brief 得到温度值
 * @return  温度值
 * */
float MPU_Get_Temperature(void)
{
    unsigned char  buf[2];
    short raw;
    float temp;
    MPU_Read_Bytes(MPU_TEMP_OUTH_REG,2,buf);
    raw=(buf[0]<<8)| buf[1];
    temp=(36.53+((double)raw)/340)*100;
//  temp = (long)((35 + (raw / 340)) * 65536L);
    return temp/100.0f;
}


/*!
 * @brief 得到陀螺仪值(原始值)
 * @param gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    uint8_t buf[6],res;
    res=MPU_Read_Bytes(MPU_GYRO_XOUTH_REG,6,buf);
    if(res==0)
    {
        *gx=((uint16_t)buf[0]<<8)|buf[1];
        *gy=((uint16_t)buf[2]<<8)|buf[3];
        *gz=((uint16_t)buf[4]<<8)|buf[5];
    }
    return res;
}



/*!
 * @brief 得到加速度值(原始值)
 * @param ax,ay,az:加速度x,y,z轴的原始读数(带符号)
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    uint8_t buf[6],res;
    res=MPU_Read_Bytes(MPU_ACCEL_XOUTH_REG,6,buf);
    if(res==0)
    {
        *ax=((uint16_t)buf[0]<<8)|buf[1];
        *ay=((uint16_t)buf[2]<<8)|buf[3];
        *az=((uint16_t)buf[4]<<8)|buf[5];
    }
    return res;;
}



